import numpy as np
from zhnbodeplot import plot_bode
index = np.linspace(-1, 3, 41)
omega = np.power(10, index)
Kp = 1
Ki = 1
Kd = 0
N = 100
pRe1 = Ki*N-(Kp+Kd)*omega**2
pIm1 = omega*(Kp*N+Ki)
pRe2 = -omega**2
pIm2 = N*omega
phi = np.arctan2(pIm1, pRe1)*180/np.pi
phi -= np.arctan2(pIm2, pRe2)*180/np.pi
amp = np.sqrt(pRe1**2 + pIm1**2)
amp /= np.sqrt(pRe2**2 + pIm2**2)

# a = 7/0.15
# b = 1/0.15
# phi -= 180
# amp *= (b/(a+omega**2))
amp *= (0.3 + 10/omega**2)
plot_bode(index, amp, phi)